//
// lunohod.cpp 
// AK 2014 04
//

#ifndef lunohod_cpp
#define lunohod_cpp

#include "lunohod.h"

//
lunohod::lunohod()
{
	maxdist = 20;
}

//
void lunohod::goLun(int a, int s){
  switch(a){
    case 0:
      forward(s);
    break;
    case 1:
      back(s);
    break;
    case 2:
      rotate(s);
    break;
	default:
	  stop(); 
	}
}

//
void lunohod::set_motor_1(int in1, int in2, int en){
  M1.in1 = in1;
  M1.in2 = in2;
  M1.en  = en;
  set_m_pins(&M1);
}

//
void lunohod::set_motor_2(int in1, int in2, int en){
  M2.in1 = in1;
  M2.in2 = in2;
  M2.en  = en;
  set_m_pins(&M2);
}

//
void lunohod::set_ultrasonic_1(int trig, int echo, int led){
  U1.trig = trig;
  U1.echo = echo;
  U1.led  = led;
  set_u_pins(&U1);
}

//
void lunohod::set_ultrasonic_2(int trig, int echo, int led){
  U2.trig = trig;
  U2.echo = echo;
  U2.led  = led;
  set_u_pins(&U2);
}

//
void lunohod::forward(int speed)
{
	m_forward(&M1,speed);
	m_forward(&M2,speed);
}

//
void lunohod::back(int speed)
{
	m_back(&M1,speed);
	m_back(&M2,speed);
}

//
void lunohod::stop()
{
	m_stop(&M1);
	m_stop(&M2);
}

//
void lunohod::rotate(int speed)  
{
  int inRand;
  inRand = random(10);
  inRand = 3;
  if(inRand > 5){
	m_forward(&M1,speed);
	m_back(&M2,speed);
  }else{
	m_forward(&M2,speed);
	m_back(&M1,speed);
  }
  delay(200);
}

//
void lunohod::set_m_pins(MOTOR *m)
{
	if(m)
	{
		pinMode(m->in1, OUTPUT);
		pinMode(m->in2, OUTPUT);
		pinMode(m->en, OUTPUT);
	}
}

//
void lunohod::set_u_pins(ULTRASONIC *u)
{
	if(u)
	{
		pinMode(u->trig, OUTPUT);
		pinMode(u->echo, INPUT);
		pinMode(u->led, OUTPUT);
	}
}

//
void lunohod::m_forward(MOTOR *m,int speed)
{
	if(m)
	{
		digitalWrite(m->in1, HIGH);
		digitalWrite(m->in2, LOW);
		analogWrite(m->en, speed);
	}
}

//
void lunohod::m_back(MOTOR *m,int speed)
{
	if(m) {
		digitalWrite(m->in1, LOW);
		digitalWrite(m->in2, HIGH);
		analogWrite(m->en, speed);
	}
}

//
void lunohod::m_stop(MOTOR *m)
{
	if(m) {
		digitalWrite(m->in1, HIGH);
		digitalWrite(m->in2, HIGH);
		digitalWrite(m->en, HIGH);
	}
}

//
long lunohod::Timing(ULTRASONIC *u)
{
 if(u) {
  digitalWrite(u->trig, LOW);
  delayMicroseconds(2);
  digitalWrite(u->trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(u->trig, LOW);
  duration = pulseIn(u->echo, HIGH);
  if(duration/29/2 < maxdist) {
    digitalWrite(u->led, HIGH);
  }else{
    digitalWrite(u->led, LOW);
  }
  return duration;
 }
}

//
long lunohod::distance(int s)
{
  switch(s){
    case 1:
	Timing(&U1);
    break;
    case 2:
	Timing(&U2);
    break;
	}
  
  distace_cm = duration /29 / 2 ;
  return distace_cm;
}


#endif // #ifndef lunohod_cpp

